#include "barohandler.h"
#include <QDebug>

/**

  */
BaroHandler::BaroHandler(QObject *parent):QObject(parent){
    referenceSet = false;
    temperature = 0;
    pressure = 0;
    referenceTemp = 0;
    referencePress = 0;
    firsttimedone = false;
    bmp085Calibration();
}

/**

  */
short BaroHandler::bmp085GetTemperature(bool *ok){
    long x1, x2;
    unsigned ut = bmp085ReadUT(ok);

    //ut = 27898; //test
    if(*ok){
        x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;
        x2 = ((long)mc << 11)/(x1 + md);
        b5 = x1 + x2;

        return ((b5 + 8)>>4); //pressure in 0.1 degree C
    }
    else{
        return 0;
    }
}

/**

  */
long BaroHandler::bmp085GetPressure(bool *ok){
    long x1, x2, x3, b3, b6, p;
    unsigned long b4, b7;
    unsigned long up = bmp085ReadUP(ok);

    if(*ok){
        qDebug() << "Calculating Pressure";
        b6 = b5 - 4000;
        // Calculate B3
        x1 = (b2 * (b6 * b6)>>12)>>11;
        x2 = (ac2 * b6)>>11;
        x3 = x1 + x2;
        b3 = (((((long)ac1)*4 + x3)<<OSS) + 2)>>2;

        // Calculate B4
        x1 = (ac3 * b6)>>13;
        x2 = (b1 * ((b6 * b6)>>12))>>16;
        x3 = ((x1 + x2) + 2)>>2;
        b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;



        b7 = ((unsigned long)up - b3) * (50000>>OSS);


        if (b7 < 0x80000000)
            p = (b7<<1)/b4;
        else
            p = (b7/b4)<<1;

        x1 = (p>>8) * (p>>8);
        x1 = (x1 * 3038)>>16;
        x2 = (-7357 * p)>>16;
        p += (x1 + x2 + 3791)>>4;

        return p; //pressure in Pa
    }else{
        return 0;
    }
}

/**

  */
int BaroHandler::getTemperature(){
    return temperature;
}

/**

  */
int BaroHandler::getPressure(){
    return pressure;
}

/**
Get raw altitude data
  */
float BaroHandler::getAltitude(){
    float alt = 0;
    if(referenceSet==true)
    {
        double x1 = (((double)referenceTemp+2731.5)/10.0)/0.0065; // Ts/kT
        double x2 = -0.1902943411; //-R*kT/g0
        double x3 = (double)pressure/(double)referencePress; //bmp085GetPressure(); // pb/ps
        qDebug() << "[BaroHandler] X1=" << x1 << " X2=" << x2 << " X3=" << x3;
        qDebug() << "[BaroHandler] RefTemp=" << referenceTemp << " referencePress=" << referencePress;
        alt = x1*(qPow(x3,x2)-1);
        qDebug() << "[BaroHandler] alt=" << alt;

    }
    else
    {
        qWarning() << "[BaroHandler ] Reference is not set";
        alt = 0;
    }
    return alt;
}

/**
  Get averaged altitude data (less noise)
  */
float BaroHandler::getAverageAltitude()
{
    float alt = getAltitude();
    if(altitudeData.size()>10)
    {
        altitudeData.removeFirst();
    }
    altitudeData.append(alt);
    return averageFromList(altitudeData);
}


/**

  */
void BaroHandler::runHandler(){
    bool test = false;
    qDebug() << "[BaroHandler] Run handler started";
    barocom.initialize();
    int tmpTemp = bmp085GetTemperature(&test);
    if(test){
        qDebug() << "[BaroHandler] Test =" << test;
        temperature = tmpTemp;
        int tmpPress = bmp085GetPressure(&test);
        if(test){
            if(firsttimedone)
            {
                if((tmpPress>(pressure-500))&&(tmpPress<(pressure+500))) //allow maximal 500 Pa ~ 40m difference between last measurement
                {
                    pressure = tmpPress;
                }
                else
                {
                    pressure = pressure;
                }
            }
            else
            {
                pressure = tmpPress;
                firsttimedone = true;
            }
        }
    }
    qDebug() << "[BaroHandler] Temperature: " << temperature;
    qDebug() << "[BaroHandler] Pressure: " << pressure;
    barocom.deInit();
    QTimer::singleShot(100, this, SLOT(runHandler()));
}

/**

  */
void BaroHandler::setReference(){
    qDebug() << "[BaroHandler] Setting Reference";
    QList<float> tempTemp;
    QList<float> tempPres;
    for(int i=0;i<10;i++)
    {
        bool test = false;
        barocom.initialize();
        int tmpTemp = bmp085GetTemperature(&test);
        if(test)
        {
            tempTemp.append(tmpTemp);
            int tmpPress = bmp085GetPressure(&test);
            if(test)
            {
                tempPres.append(tmpPress);
                qDebug() << "[BaroHandler] reference tmp values" << tmpTemp << tmpPress << i;
            }
        }
        barocom.deInit();
        Sleeper::msleep(100);
    }

    referenceTemp = averageFromList(tempTemp);
    referencePress = averageFromList(tempPres);
    referenceSet = true;
    qDebug() << "[BaroHandler] Reference is set reftemp= " << referenceTemp << "refpress =" << referencePress;
}

/**

  */
int BaroHandler::bmp085ReadInt(unsigned char address, bool *ok){
    qDebug() << "[BaroHandler] Reading Integer";
    unsigned char msb = 0;
    unsigned char lsb = 0;
    QByteArray tmp = barocom.readI2c(BMP085_ADDRESS,address,2);
    if(tmp.size() >= 2)
    {
        msb = tmp.at(0);
        lsb = tmp.at(1);
        if(ok)
            *ok = true;
    }
    else
    {
        qWarning() << "[BaroHandler] Bytearray size is less than 2";
        if(ok)
            *ok = false;
        qWarning() << "[BaroHandler]" << *ok;
    }
    qDebug() << "msb" << msb;
    qDebug() << "lsb" << lsb;

    return (qint16) (msb<<8 | lsb);
}

/**

  */
void BaroHandler::bmp085Calibration(){
    ac1 = bmp085ReadInt(0xAA);
    ac2 = bmp085ReadInt(0xAC);
    ac3 = bmp085ReadInt(0xAE);
    ac4 = bmp085ReadInt(0xB0);
    ac5 = bmp085ReadInt(0xB2);
    ac6 = bmp085ReadInt(0xB4);
    b1 = bmp085ReadInt(0xB6);
    b2 = bmp085ReadInt(0xB8);
    mb = bmp085ReadInt(0xBA);
    mc = bmp085ReadInt(0xBC);
    md = bmp085ReadInt(0xBE);
}

/**

  */
int BaroHandler::bmp085ReadUT(bool *ok){
    unsigned int ut;
    char addr = 0x2E;
    barocom.writeI2c(BMP085_ADDRESS,0xF4,&addr,1);

    Sleeper::msleep(5); // sleep while the sensor converts the reading

    ut = bmp085ReadInt(0xF6, ok);

    return ut;
}

/**

  */
long BaroHandler::bmp085ReadUP(bool *ok){
    unsigned char msb, lsb, xlsb;
    unsigned long up = 0;
    char data = 0x34+(OSS<<6);
    barocom.writeI2c(BMP085_ADDRESS,0xF4,&data, 1);

    Sleeper::msleep(25); // sleep while the sensor converts the reading; time depending on OSS

    QByteArray tmp  = barocom.readI2c(BMP085_ADDRESS,0xF6,3);
    if(tmp.size() >= 3){
        msb = tmp.at(0);
        lsb = tmp.at(1);
        xlsb = tmp.at(2);
        if(ok){
            *ok = true;
        }

    }
    else
    {
        msb = 0;
        lsb = 0;
        xlsb = 0;
        if(ok){
            *ok = false;
        }
        qWarning() << "[BaroHandler] Bytearray size is less than 3";
        qWarning() << "[BaroHandler] test = " << *ok;
    }
    up = (((unsigned long) msb << 16) | ((unsigned long) lsb << 8) | (unsigned long) xlsb) >> (8-OSS);
    return up;
}

float BaroHandler::averageFromList(QList<float> input)
{
    float sum = 0.0;
    for(int i=0; i < input.size(); i++){
        sum += input.at(i);
    }
    return (float)sum/(float)input.size();
}
